#include "Matcher.h"
#include "Util.h"

using namespace cv;

#define SHOW_MATCH false

//Copy (x,y) location of descriptor matches found from KeyPoint data structures into Point2f vectors
void matches2points_t(const vector<DMatch>& matches, const vector<KeyPoint>& kpts_train,
	const vector<KeyPoint>& kpts_query, vector<Point2f>& pts_train, vector<Point2f>& pts_query)
{
	pts_train.clear();
	pts_query.clear();
	pts_train.reserve(matches.size());
	pts_query.reserve(matches.size());
	for (size_t i = 0; i < matches.size(); i++)
	{
		const DMatch& match = matches[i];
		pts_query.push_back(kpts_query[match.queryIdx].pt);
		pts_train.push_back(kpts_train[match.trainIdx].pt);
	}

}

void crossCheckMatching( Ptr<DescriptorMatcher>& descriptorMatcher,
	const Mat& descriptors1, const Mat& descriptors2,
	vector<DMatch>& filteredMatches12, int knn=1 )
{
	filteredMatches12.clear();
	vector<vector<DMatch> > matches12, matches21;
	descriptorMatcher->knnMatch( descriptors1, descriptors2, matches12, knn );
	descriptorMatcher->knnMatch( descriptors2, descriptors1, matches21, knn );
	for( size_t m = 0; m < matches12.size(); m++ )
	{
		bool findCrossCheck = false;
		for( size_t fk = 0; fk < matches12[m].size(); fk++ )
		{
			DMatch forward = matches12[m][fk];

			for( size_t bk = 0; bk < matches21[forward.trainIdx].size(); bk++ )
			{
				DMatch backward = matches21[forward.trainIdx][bk];
				if( backward.trainIdx == forward.queryIdx )
				{
					filteredMatches12.push_back(forward);
					findCrossCheck = true;
					break;
				}
			}
			if( findCrossCheck ) break;
		}
	}
}

CImageMatcher::CImageMatcher() : m_bShowImages(SHOW_MATCH)
{
}

CImageMatcher::~CImageMatcher()
{
}

void CImageMatcher::ToggleShowMatches()
{
	m_bShowImages = !m_bShowImages;
}

bool CImageMatcher::IsShowMatches() const
{
	return m_bShowImages;
}

MatchedImages* CImageMatcher::MatchFromFile(ImagesVector& Images, char* fileName)
{
	FILE* f = fopen(fileName, "r");

	MatchedImages* pMatched = new MatchedImages;

	UINT numMatches = 0;
	fscanf(f, "%d\n", &numMatches);
	for (UINT i = 0; i < min(numMatches, Images.size() - 1); i++)
	{
		int idx1, idx2, numPoints;
		fscanf(f, "%d %d %d\n", &idx1, &idx2, &numPoints);
		PointVector points1; 
		PointVector points2; 
		for (int j = 0; j < numPoints; j++)
		{
			double im1Point[2], im2Point[2];
			fscanf(f, "%lf %lf %lf %lf\n", im1Point, im1Point+1, im2Point, im2Point + 1);
			Point2d p1(im1Point[0], im1Point[1]);
			Point2d p2(im2Point[0], im2Point[1]);
			points1.push_back(p1);
			points2.push_back(p2);
		}
		CImageMatch* newMatch = NULL;//new CImageMatch(idx1, idx2);
		//newMatch->SetMatchingPoints(points1, points2, numPoints);
		pMatched->push_back(newMatch);
	}

	return pMatched;
}

CImageMatch* CImageMatcher::MatchSinglePair(Mat im1, Mat im2, int FirstImageIndex, int SecondImageIndex)
{
	const float ransacReprojThreshold = 3;
	char csWinName[70];
	sprintf(csWinName, "Image Matching %d - %d", FirstImageIndex, SecondImageIndex);
	const string winName(csWinName);
	Ptr<FeatureDetector> detector = FeatureDetector::create( "SIFT" );
	Ptr<DescriptorExtractor> descriptorExtractor = DescriptorExtractor::create( "SIFT" );
	Ptr<DescriptorMatcher> descriptorMatcher = DescriptorMatcher::create( "BruteForce" );

	std::vector<KeyPoint> kpts_1, kpts_2;
	detector->detect(im1, kpts_1);
	detector->detect(im2, kpts_2);

	//extracting descriptors from keypoints using sift
	Mat desc_1, desc_2;
	descriptorExtractor->compute(im1, kpts_1, desc_1);
	descriptorExtractor->compute(im2, kpts_2, desc_2);

	//matching descriptors
	vector<DMatch> filteredMatches;
	crossCheckMatching( descriptorMatcher, desc_1, desc_2, filteredMatches, 1 );

	vector<int> queryIdxs( filteredMatches.size() ), trainIdxs( filteredMatches.size() );
	for( size_t i = 0; i < filteredMatches.size(); i++ )
	{
		queryIdxs[i] = filteredMatches[i].queryIdx;
		trainIdxs[i] = filteredMatches[i].trainIdx;
	}


	// Computing homography (RANSAC)
	vector<Point2f> points1; 
	vector<Point2f> points2; 
	KeyPoint::convert(kpts_1, points1, queryIdxs);
	KeyPoint::convert(kpts_2, points2, trainIdxs);
	Mat H12 = findHomography( Mat(points1), Mat(points2), CV_RANSAC, ransacReprojThreshold );
	cout << ">" << endl;


	Mat drawImg;
	if( !H12.empty() ) // filter outliers
	{
		vector<char> matchesMask( filteredMatches.size(), 0 );
		Mat points1t; perspectiveTransform(Mat(points1), points1t, H12);
		for( size_t i1 = 0; i1 < points1.size(); i1++ )
		{
			if( norm(points2[i1] - points1t.at<Point2f>((int)i1,0)) < 4 ) // inlier
				matchesMask[i1] = 1;
		}
		if (m_bShowImages)
		{
			// draw inliers
			drawMatches( im1, kpts_1, im2, kpts_2, filteredMatches, drawImg, CV_RGB(0, 255, 0), CV_RGB(0, 0, 255), matchesMask);
		}
	}
	else
	{
		VISIO_ASSERT(0);
	}

	if (m_bShowImages)
	{
		namedWindow(winName, 1);
		imshow( winName, drawImg );
	}

	//CImageMatch* pNewMatch = new CImageMatch(FirstImageIndex,SecondImageIndex);
	//pNewMatch->SetMatchingPoints(points1, points2, filteredMatches.size());
	//return pNewMatch;
	return NULL;
}

MatchedImages* CImageMatcher::Match(ImagesVector& Images )
{

	MatchedImages* pMatched = new MatchedImages;

	Mat FirstImage = Images[0];
	for (UINT i = 1; i < Images.size(); i++)
	{
		CImageMatch* pNewMatch = MatchSinglePair(FirstImage, Images[i], 0 , i);
		pMatched->push_back(pNewMatch);
	}
	//detecting keypoints 

	return pMatched;

}
